import smbus
import time
import serial
import logging


class PCA9685:
    # ... (保留原有寄存器定义和初始化代码不变)

    def set_servo_angle(self, channel, angle):
        """优化：仅当角度变化超过阈值时才更新 PWM"""
        if hasattr(self, f'_last_angle_{channel}'):
            last_angle = getattr(self, f'_last_angle_{channel}')
            if abs(angle - last_angle) < 1.0:  # 角度变化小于1度时跳过
                return

        pulse_width_us = (angle / 180.0) * (2500 - 500) + 500
        pulse = int(pulse_width_us * 4096 / 20000)
        self.set_pwm(channel, 0, pulse)
        setattr(self, f'_last_angle_{channel}', angle)  # 记录当前角度
        time.sleep(0.02)  # 缩短延时但仍保证舵机响应
        self.logger.info(f"Set servo angle for channel {channel}: {angle} degrees")


class PanTiltController:
    def __init__(self, horizontal_min, horizontal_max, vertical_min, vertical_max, center_threshold,
                 kp_pan, ki_pan, kp_tilt, ki_tilt, alpha, camera_flipped=False, debug=False):
        # ... (保留原有初始化代码)
        self.last_update_time = time.time()
        self.idle_timeout = 2.0  # 无操作超时时间（秒）

    def control(self):
        ser = serial.Serial(
            port='/dev/ttyS0',
            baudrate=9600,
            timeout=1,
            parity=serial.PARITY_NONE,
            stopbits=serial.STOPBITS_ONE,
            bytesize=serial.EIGHTBITS
        )
        try:
            while True:
                current_time = time.time()
                has_data = False

                if ser.in_waiting > 0:
                    buffer = ser.read(ser.in_waiting)
                    if b'#' in buffer:
                        parts = buffer.split(b'#', 1)
                        raw_data = parts[0] + b'#'
                        x, y = self.parse_data(raw_data)
                        if x is not None and y is not None:
                            has_data = True
                            self.last_update_time = current_time

                            # 应用低通滤波
                            x_filtered = self.alpha * x + (1 - self.alpha) * 0
                            y_filtered = self.alpha * y + (1 - self.alpha) * 0

                            # 水平控制（带死区检测）
                            if abs(x_filtered) > self.center_threshold:
                                self.integral_x += x_filtered
                                self.integral_x = max(min(self.integral_x, 500), -500)
                                pan_adjust = x_filtered * self.kp_pan + self.integral_x * self.ki_pan
                                new_pan = self.current_pan - pan_adjust
                                new_pan = max(min(new_pan, self.horizontal_max), self.horizontal_min)
                                if abs(new_pan - self.current_pan) >= 1.0:  # 变化超过1度才更新
                                    self.current_pan = new_pan
                                    self.pca9685.set_servo_angle(9, self.current_pan)
                                    self.logger.info(f"Pan adjusted to: {self.current_pan:.1f}")

                            # 垂直控制（带死区检测）
                            if abs(y_filtered) > self.center_threshold:
                                self.integral_y += y_filtered
                                self.integral_y = max(min(self.integral_y, 500), -500)
                                tilt_adjust = y_filtered * self.kp_tilt + self.integral_y * self.ki_tilt
                                new_tilt = self.current_tilt + tilt_adjust
                                new_tilt = max(min(new_tilt, self.vertical_max), self.vertical_min)
                                if abs(new_tilt - self.current_tilt) >= 1.0:  # 变化超过1度才更新
                                    self.current_tilt = new_tilt
                                    self.pca9685.set_servo_angle(10, self.current_tilt)
                                    self.logger.info(f"Tilt adjusted to: {self.current_tilt:.1f}")

                # 无数据时的归中逻辑（带超时检测）
                if not has_data and (current_time - self.last_update_time) > self.idle_timeout:
                    if abs(self.current_pan - 90) > 1.0 or abs(self.current_tilt - 90) > 1.0:
                        delta_pan = (90 - self.current_pan) * 0.2  # 降低归中速度
                        delta_tilt = (90 - self.current_tilt) * 0.2
                        self.current_pan += delta_pan
                        self.current_tilt += delta_tilt
                        self.pca9685.set_servo_angle(9, int(self.current_pan))
                        self.pca9685.set_servo_angle(10, int(self.current_tilt))
                        self.logger.debug(f"Homing: Pan={int(self.current_pan)}, Tilt={int(self.current_tilt)}")

                time.sleep(0.05)  # 主循环固定延时

        except KeyboardInterrupt:
            self.logger.info("Program terminated")
        finally:
            # 关机时重置所有通道
            for ch in range(16):
                self.pca9685.set_pwm(ch, 0, 0)
